#!/usr/bin/env python
#coding:utf-8
#roslaunch turtlebot_teleop keyboard_teleop.launch

import struct 
import binascii
import serial
import math

import rospy

from geometry_msgs.msg import Twist


#typedef struct hd_robot_protocol
#{
#	int8_t type[3];			//the type of device, example dq2 
#	int8_t move_mode;			//move mode:0-linear motion,1-rotation motion,2-stop
#	int16_t start_dir;			//Direction of departure:0~360(-180 ~ 180)
#	uint16_t speed;			//line speed:mm/s,max  65535mm/s
#	uint16_t time;				//moving time:ms, max: 65535ms
#}hd_robot_protocol;
#discovery_q2 = struct.Struct('<3c1bhHH')

#discovery_q2 = struct.Struct('<3c1bhHH')

class HDDiscoveryQ2:
	def __init__(self):
		self.q2 = struct.Struct('<3sbhHHBHB')
		self.type		= 'dq2'
		self.move_mode	= 0
		self.start_dir	= 0
		self.speed		= 0
		self.time		= 0
		self.unused		= 0
		self.verge		= 0
		self.end		= 0xaa

	def pack(self):
		data = self.q2.pack( self.type, self.move_mode, \
							 self.start_dir, self.speed, self.time, \
							 self.unused, 0, 0 )
		
		checksum = 0
		for i in range( len(data) ):
			x, = struct.unpack('<B', data[i])
			checksum+=int(x)
		self.verge = checksum
		return self.q2.pack( self.type, self.move_mode, \
							 self.start_dir, self.speed, self.time, \
							 self.unused, self.verge, self.end )


def stop():
	#stop the q2 platform
	a = 0
	



#current status




def hd_q2_callback(msg):
	global before
	#the format of msg refer to: http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/Twist.html
	#msg.linear.x
	#msg.linear.y
	#msg.linear.z

	#msg.angular.x
	#msg.angular.y
	#msg.angular.z
	
	now = rospy.Time.now()
	duration = now - before

	#print duration.secs, duration.nsecs / (1000*1000)

	if duration.secs <= 0 and ( duration.nsecs/(1000*1000) ) <= 100:
		return 
	
	before = rospy.Time.now()

	rospy.loginfo("Received a /cmd_vel message!")
	rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z))
	rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z))

	#stop
	if     msg.linear.x  == 0.0 \
	   and msg.linear.y  == 0.0 \
	   and msg.angular.x == 0.0 \
	   and msg.angular.z == 0.0 :
	   
		q2 = HDDiscoveryQ2()
		q2.move_mode = 2
		serial_port.write( q2.pack() )
		return 

	#rotation
	#目前自转部分因为Q2没有提供自转的接口，所以部分代码需要等到串口功能正常（或联调的时候）
	#才编写、测试。
	
	if msg.angular.z != 0.0:
		q2 = HDDiscoveryQ2()
		q2.move_mode = 1
		if msg.angular.z < 0.0:
			q2.start_dir = 0
		else:
			q2.start_dir = 1
		q2.speed = int( abs(msg.angular.z) * 200)
		q2.time  = 50
		serial_port.write( q2.pack() )
		return 
		
	
	#straight
	#About the direction
	#x = 0  (y=0 and x > 0)
	#x = 180 (y=0 and x < 0)
	#x = 90 pi (x=0 & y>0 )
	#x = 270 pi (x=0 & y<0 )
	#x = arctan( y/x ) ( y>0  )
	#x = 180 + arctan( y/x ) < y<0 )

	if msg.linear.y == 0.0:
		if msg.linear.x >= 0.0:
			x = 90
			#print 'UP'
		else:
			#print 'Down'
			x = 270
		q2 = HDDiscoveryQ2()
		q2.move_mode = 0
		q2.start_dir = x
		speed = abs(msg.linear.x)
		
		#print speed
		
		q2.speed	= int(1000*speed)
		if q2.speed >= 2.5*1000:
			q2.speed = 2.5*1000
		q2.time		= 100
		serial_port.write( q2.pack() )
		return 

	if msg.linear.x == 0.0:
		if msg.linear.y > 0.0:
			x = 180
		else:
			x = 0
		q2 = HDDiscoveryQ2()
		q2.move_mode = 0
		q2.start_dir = x
		q2.speed     = 100
		if q2.speed >= 2.5*1000:
			q2.speed = 2.5*1000
		q2.time      = 250
		serial_port.write( q2.pack() )
		return 
			
	
	_x = math.atan( msg.linear.y / msg.linear.x )
	if _x < 0.0:
		_x += math.pi
	x = _x * 180 / math.pi
	x = int(x)
	
	if msg.linear.y < 0.0:
		x = x + 180
	
	q2 = HDDiscoveryQ2()
	q2.move_mode = 0
	q2.start_dir = x
	q2.speed	 = 100
	if q2.speed >= 3*1000:
		q2.speed = 3*1000
	q2.time	  = 500
	serial_port.write( q2.pack() )
	
	return 


if __name__ == '__main__':
	global serial_port
	global slow
	global before
	
	
	slow = 0
	rospy.init_node('discovery_q2')

	before = rospy.Time.now()

	rospy.Subscriber( "/cmd_vel_mux/input/teleop" , Twist, hd_q2_callback )

	serial_port = serial.Serial( '/dev/discovery_q2', 9600 )

	#port = rospy.getParam("port_name")
	rospy.spin()
	
	
